#ifndef __UTILITY_FUNCTIONS__
#define __UTILITY_FUNCTIONS__

#include <iostream>


template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down2(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());
  
  index j;
  while (p.last_child(i) <= p.last_leaf()) {
    j = p.top_all_present(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
    i = j;
  }
  
  while(p.first_child(i) <= p.last_leaf()) {
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i, less);	
    else 
      j = p.top_some_absent(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
      i = j;
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
  }    
  p.update_all_present(a, i, x, less);
  return;  

  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

}


template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_up(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());
  
  index j, k;
  position ak;
  while(1) {
    index m = (i / 31) * 31;
    ak = a + m;
    j = i;
    i = i - m;
    k = (i - 1) >> 1;

  while(k) {
    if (less(x, *(ak + k))) {
      *(ak + i) = x;
      return;
    }
    else {
      i = k;
      k = (k - 1) >> 1;
      *(ak + i) = *(ak + k);
    }
  }

  if(less(x, *(ak))) {
    *(ak + i) = x;
    return;
  }
  else {
    if(m != 0) {
      m = ((m / 31) - 1) >> 5;
      i = ((30 * m) + 29 + (j / 31)) >> 1;
      m = (i + ((i / 31) * 31) - 1) >> 1;
	*(ak) = *(a + i);
	if(less(x, *(a + i))) {
	  *(ak) = x;
	  return;
	}
	*(a + i) = *(a + m);
      }
      else {
	*(ak) = x;
	return;
      } 
    }
  }
}


template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down1(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());

  index j = (i / 31) * 31;
  
  if((j != i) && (p.last_leaf() >= (j + 30))) {
    index l = ((((p.last_leaf() / 31) - 1) / 31) + 1) * 31;
    if(i >= l && (i > (j + 14)))
      return;
    index k = j + 15;
    while (i < k) {
      j = p.top_all_present(a, i, less);
      if(less(x, *(a + j))) 
	*(a + i) = *(a + j);    
      else {
	*(a + i) = x;
	return;
      }
      i = j;
    }
    if(i > l) {
      *(a + i) = x;
      return;
    }
    else {
      if(p.last_child(i) <= p.last_leaf())
	j = p.top_all_present(a, i, less);
      else {
	if(p.first_child(i) <= p.last_leaf())
	  j = p.top_some_absent(a, i, less);
	else {
	  *(a + i) = x;    
	  return;
	}
      }
      if  (less(x, *(a + j))) 
	*(a + i) = *(a + j);    
      else {
	*(a + i) = x;
	return;
      }
      i = j;
    }
  }

  index m = 31 * (p.last_leaf() / 992);
  while (i < m) {
    index k = i;
    position ak = a + i;
    i = (less(*(ak + 1), *(ak + 2))) ? 2 : 1;
    *(ak) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;    
    *(ak + j) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;    
    *(ak + j) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;    
    *(ak + j) = *(ak + i);
    j = i;
    i += k;
    i = (62 * i) - 899 - (930 * (k / 31));
    //i = 14 * (((k / 7) << 2) + i - 2);
    if(less(*(a + i), *(a + i + 31))) i += 31;  
    if(less(*(a + i), x)) {
      if(less(*(ak + j), x)) {
	if(less(*(ak + ((j - 3) >> 2)), x)) {
	  *(ak + ((j - 1) >> 1)) = *(ak + ((j - 3) >> 2)); 
	  if(less(*(ak + ((j - 7) >> 3)), x)) {
	    *(ak + ((j - 3) >> 2)) = *(ak + ((j - 7) >> 3));	    
	    if(less(*(ak), x)) {
	      *(ak + ((j - 7) >> 3)) =  *(ak);
	      *(ak) = x;
	    }
	    else
	      *(ak + ((j - 7) >> 3)) =  x;     
	  }
	  else
	    *(ak + ((j - 3) >> 2)) = x;	  
	}
	else
	  *(ak + ((j - 1) >> 1)) = x;
      }
      else
	*(ak + j) =  x;
      return;
    }
    else
      *(ak + j) =  *(a + i);
  }
  if ((((992 * (i / 31)) + 31) > p.last_leaf()) && ((i + 30) <= p.last_leaf())) {
    position ak = a + i;
    i = (less(*(ak + 1), *(ak + 2))) ? 2 : 1;
    if(less(*(ak + i), x)) {
      *(ak) = x;
      return;
    }
    *(ak) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;
    if(less(*(ak + i), x)) {
      *(ak + j) = x;
      return;
    }     
    *(ak + j) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;    
    if(less(*(ak + i), x)) {
      *(ak + j) = x;
      return;
    }   
    *(ak + j) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;    
    if(less(*(ak + i), x)) {
      *(ak + j) = x;
      return;
    }
    *(ak + j) = *(ak + i);
    *(ak + i) = x;
    return;
  }
  if((i != 0) && (m == i)) {

    index k = i + 15;
    while (i < k) {
      j = p.top_all_present(a, i, less);
      if(less(x, *(a + j))) 
	*(a + i) = *(a + j);    
      else {
	*(a + i) = x;
	return;
      }
      i = j;
    }
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i,  less);
    else {
      if(p.first_child(i) <= p.last_leaf())
	j = p.top_some_absent(a, i, less);
      else {
	*(a + i) = x;
	return;
      }
    }
    if(less(x, *(a + j))) 
      *(a + i) = *(a + j);    
    else {
      *(a + i) = x; 
      return;
    }
    i = j;
  }
  
  sift_down2<d, position, index, element, ordering, policy>(a, i, x, less, p);
   
   

  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

}

template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());

  index j;

  index m = 31 * (p.last_leaf() / 992);
  while (i < m) {
    index k = i;
    position ak = a + i;
    i = (less(*(ak + 1), *(ak + 2))) ? 2 : 1;
    *(ak) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;    
    *(ak + j) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;    
    *(ak + j) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;    
    *(ak + j) = *(ak + i);
    j = i;
    i += k;
    i = (62 * i) - 899 - (930 * (k / 31));
    //i = 14 * (((k / 7) << 2) + i - 2);
    if(less(*(a + i), *(a + i + 31))) i += 31;  
    *(ak + j) =  *(a + i);
  }
  if ((((992 * (i / 31)) + 31) > p.last_leaf()) && ((i + 30) <= p.last_leaf())) {
    m = i;
    position ak = a + i;
    i = (less(*(ak + 1), *(ak + 2))) ? 2 : 1;
    *(ak) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;
    *(ak + j) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;    
    *(ak + j) = *(ak + i);
    j = i;
    i = (i << 1) + 1;
    if(less(*(ak + i), *(ak + i + 1))) ++i;    
    *(ak + j) = *(ak + i);
    m = m + i;
    sift_up<d, position, index, element, ordering, policy>(a, m, x, less, p);
    return;
  }

  if((i != 0) && (m == i)) {

    index k = i + 15;
    while (i < k) {
      j = p.top_all_present(a, i,  less);
      *(a + i) = *(a + j);    
      i = j;
    }
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i,  less);
    else {
      if(p.first_child(i) <= p.last_leaf())
	j = p.top_some_absent(a, i, less);
      else {
	sift_up<d, position, index, element, ordering, policy>(a, i, x, less, p);
	return;
      }
    }
    *(a + i) = *(a + j);    
    i = j;
  }

  if(less(*(a + i), x) && (i)) {
    m = ((i / 31) - 1) >> 5;
    i = ((30 * m) + 29 + (i / 31)) >> 1;
    m = (i + ((i / 31) * 31) - 1) >> 1;
    *(a + i) = *(a + m);
    sift_up<d, position, index, element, ordering, policy>(a, i, x, less, p);
    return;
  }


  sift_down2<d, position, index, element, ordering, policy>(a, i, x, less, p);

      
  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

}
 

#endif // __UTILITY_FUNCTIONS__


